#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include <time.h>
#include <vector>

typedef struct _ampd_data_ {
    float *data;
    int len;
    float avg;
    int hpWin_min;
    float **peaks;
    int peakCnt;
}AmpdData;

typedef struct _data_node_ {
    uint64_t tsMs;
    float value;
}DataNode;

class PeakDetector
{
public:
    PeakDetector(int period) :
        _len(0),
        _sum(0.0),
        _avg(0.0),
        positive(0){
        hpWin_min = period/4;     // 12 * 10; // ms
        this->posPeak.value = this->negPeak.value = 0.0;
    }
    ~PeakDetector() { }

    void prepareDetector(const std::vector<DataNode> &samp) {
        int i, len;
        len = samp.size();
        for (i = 0; i < len; i++) {
            this->_sum += samp[i].value;
            if (this->posPeak.value > samp[i].value) {
                this->posPeak = samp[i];
            }
            if (this->negPeak.value < samp[i].value) {
                this->negPeak = samp[i];
            }
        }
        this->_len += len;
        this->_avg = this->_sum / this->_len;

        if (samp[i-1].value > this->_avg) {
            this->positive = 1;
        } else {
            this->positive = -1;
        }

        this->staPoint = samp[i-1];
    }
    void cleanDetector(std::vector<DataNode> &peaks) {
        if ((this->lastPoint.tsMs - staPoint.tsMs) < this->hpWin_min) {
            return;
        }
        if (positive == 1) {
            if (fabs((this->posPeak.value - this->_avg) / this->_avg) < 0.001) {
                return;
            }
            peaks.push_back(posPeak);
        } else {
            if (fabs((this->_avg - this->negPeak.value) / this->_avg) < 0.001) {
                return;
            }
            peaks.push_back(negPeak);
        }
        printf("\n");
    }
    void detectPeaks(const std::vector<DataNode> &samp, std::vector<DataNode> &peaks) {
        int i, len;
        len = samp.size();
        if (this->_len > 7200*10) {   // 波形限制，最大周期长度内，未找到明显波动波形
            this->_avg = this->_sum / this->_len;
            this->_sum = 0.0;
            this->_len = 0;
        }
        for (i = 0; i < len; i++) {
            this->lastPoint = samp[i];
            if (this->positive == 1) {
                if (this->posPeak.value < samp[i].value) {  // 查找当前正峰值
                    this->posPeak = samp[i];
                }
                do {
                    if (samp[i].value >= this->_avg) break; // 正周期继续
                    if (fabs((this->posPeak.value - this->_avg) / this->_avg) < 0.001) break;   // 波形幅值限制，幅值不明显，不认为是有效波动

                    if ((this->lastPoint.tsMs - staPoint.tsMs) > this->hpWin_min) {
                        peaks.push_back(posPeak);
                        staPoint = samp[i];
                    } else {
                    }
                    this->positive = -1;        // 切换波峰符号
                    this->negPeak = samp[i];
                } while(0);
            } else {
                if (this->negPeak.value > samp[i].value) {  // 查找当前负峰值
                    this->negPeak = samp[i];
                }
                do {
                    if (samp[i].value <= this->_avg) break; // 负周期继续
                    if (fabs((this->_avg - this->negPeak.value) / this->_avg) < 0.001) break;   // 波形幅值限制，幅值不明显，不认为是有效波动

                    if ((this->lastPoint.tsMs - staPoint.tsMs) > this->hpWin_min) {
                        peaks.push_back(negPeak);
                        staPoint = samp[i];
                    } else {
                    }
                    this->positive = 1;        // 切换波峰符号
                    this->posPeak = samp[i];
                    this->_avg = this->_sum / this->_len;   // 计算上一个周期均值
                    this->_sum = 0.0;                       // 准备下一个周期累计
                    this->_len = 0;
                } while(0);
            }
            this->_sum += samp[i].value;    // 累计当前周期
            this->_len++;
        }
    }

    void debug() {
        printf("\nwin: %ld; avg: %.3f %.3f %.3f; positive: %d\n", hpWin_min, _avg, this->posPeak.value, this->negPeak.value, this->positive);
    }
private:
    int _len;               // 当前周期累计计数
    float _sum;             // 当前周期累计值
    float _avg;             // 上一周期平均值
    int positive;           // 1： 当前检测正周期；-1： 当前检测负周期
    uint64_t hpWin_min;     // ms 最小检测窗口尺寸（4分之一波）
    DataNode posPeak, negPeak, staPoint, lastPoint;
};

void ampd(AmpdData *samp)
{
    int i, sta = 0;
    int positive = 0;
    float sum = 0;
    float *posPeak, *negPeak;
    
    for (i = 0; i < samp->len; i++) {
        sum += samp->data[i];
    }
    samp->avg = sum / samp->len;
    
    if (samp->data[0] > samp->avg) {
        positive = 1;
    } else {
        positive = -1;
    }

    posPeak = negPeak = &samp->data[0];

    for (i = 0; i < samp->len; i++) {
        if (positive == 1) {
            if (*posPeak < samp->data[i]) {
                posPeak = &samp->data[i];
            }
            if (samp->data[i] < samp->avg) {
                if ((i - sta) > samp->hpWin_min) {
                    samp->peaks[samp->peakCnt] = posPeak;
                    samp->peakCnt++;
                    sta = i;
                } else {
                }
                positive = -1;
                negPeak = &samp->data[i];
            }
        } else {
            if (*negPeak > samp->data[i]) {
                negPeak = &samp->data[i];
            }
            if (samp->data[i] > samp->avg) {
                if ((i - sta) > samp->hpWin_min) {
                    samp->peaks[samp->peakCnt] = negPeak;
                    samp->peakCnt++;
                    sta = i;
                } else {
                }
                positive = 1;
                posPeak = &samp->data[i];
            }
        }
    }
    if (positive == 1) {
        samp->peaks[samp->peakCnt] = posPeak;
        samp->peakCnt++;
    } else {
        samp->peaks[samp->peakCnt] = negPeak;
        samp->peakCnt++;
    }
}

int main(int argc, char *argv[])
{
    AmpdData samp;
    int i;
    float noise = 0;
    int period, cycle, N;
    
    period = 20;
    cycle = 2;
    N = period*cycle;

    samp.len = N;
    samp.data = (float*)malloc(samp.len * sizeof(float));
    samp.peaks = (float**)malloc(samp.len / 4 * sizeof(float));
    samp.peakCnt = 0;
    samp.hpWin_min = period/4;

    srand((unsigned)time( NULL ));
    for (i = 0; i < N; i++) {
        noise = ((float)rand() / RAND_MAX - 0.5f) * 0.2f;
        samp.data[i] = 5.0f * sin(2.0*M_PI*cycle / N * i) + noise;
    }

    ampd(&samp);

    for (i = 0; i < samp.peakCnt; i++) {
        printf("%p; %02d; %.3f\n", samp.peaks[i], (int)((samp.peaks[i]-samp.data)), *samp.peaks[i]);
    }

    PeakDetector peakDtor(period*10);
    std::vector<DataNode> sampData;
    std::vector<DataNode> peaks;
    DataNode node;

    for (i = 0; i < N; i++) {
        noise = ((float)rand() / RAND_MAX - 0.5f) * 0.2f;
        node.value = 5.0f * sin(2.0*M_PI*cycle / N * i) + noise;
        node.tsMs = 10*i;
        sampData.push_back(node);
    }

    peakDtor.prepareDetector(sampData);
    peakDtor.debug();

    for (i = 0; i < N; i++) {
        sampData[i].tsMs = 10*i+10*N;
    }
    for (i = 0; i < N; i++) {
        peakDtor.detectPeaks({sampData[i]}, peaks);
    }
    for (i = 0; i < N; i++) {
        sampData[i].tsMs = 10*i+10*N*2;
    }
    peakDtor.detectPeaks(sampData, peaks);

    peakDtor.cleanDetector(peaks);
    peakDtor.debug();
    for (auto &p : peaks) {
        printf("p: %02ld; %.3f\n", p.tsMs, p.value);
    }

    return 0;
}
